#!/usr/bin/python
import roslib; roslib.load_manifest('DelphinROSv2')
import rospy
import sys
import serial
import time
import numpy
import datetime
import select
from std_msgs.msg import String
from std_msgs.msg import Int8
from DelphinROSv2.msg import tail_setpoints
from DelphinROSv2.msg import tail_feedback
from DelphinROSv2.msg import status

def setupSerial():
    global serialPort

    serialPort = serial.Serial(port='/dev/usbtail', baudrate='38400', timeout=0.2)
    serialPort.bytesize = serial.EIGHTBITS
    serialPort.stopbits = serial.STOPBITS_ONE
    serialPort.parity = serial.PARITY_NONE
    print "Setup serial port for tail section"
    print serialPort.portstr
    time.sleep(5)
    return serialPort.isOpen()

def tailAlive():
    #checks that the tail section is alive and kicking
    
    global serialPort
    message = 'b080c070d080e070f094'
    
    try:
        serialPort.write(message)       
    except ValueError,  select.error:
        return False

    # checks that feedback is given when a message is sent 
    try:
        feedback = serialPort.read(25)
    except ValueError:
        return False
    except select.error:
        return False
    
    if feedback[-1] == 'a':
        return True
    else:
        return False
    
def angleToSetpoint(angle, zero):
    maxlimit = 130
    minlimit = 30
    
    print 'angle',angle
    setpoint = zero - (1.832 * angle)
    
    #check range is within max/min
    if setpoint > maxlimit:
        setpoint = maxlimit
    elif setpoint < minlimit:
        setpoint = minlimit
    
    print 'setpoint', setpoint	
    return int(setpoint)

def speedToSetpoint(speed):

    minSpeed = 10
    if abs(speed) < minSpeed:
	speed = 0

    maxlimit = 130
    minlimit = 50
    
    setpoint = 94 - speed
    
    #check range is within max/min
    if setpoint > maxlimit:
        setpoint = maxlimit
    elif setpoint < minlimit:
        setpoint = minlimit
   
    return int(setpoint)
    
def horizontal_callback(new_angles):
    global message
    global c_starb
    global e_port
    
    c_starb = angleToSetpoint(new_angles.cs0, 70)
    e_port  = angleToSetpoint(-new_angles.cs0, 70)
    
def vertical_callback(new_angles):
    global message
    global b_top
    global d_bottom
        
    b_top    = angleToSetpoint(-new_angles.cs0, 80)
    d_bottom = angleToSetpoint(new_angles.cs1, 80)
       
    
def prop_callback(new_prop):
    global prop
    global prop_demand
    
    prop_demand = new_prop.data
    print 'new prop data arrived'
    prop = speedToSetpoint(prop_demand)
    


def tail_section_loop():
    global message
    #message = 'b090c090d090e090f094'
    global b_top
    global c_starb
    global d_bottom
    global e_port
    global prop
    
    
    b_top = 80
    c_starb = 70
    d_bottom = 80
    e_port = 70
    prop = 94
    
    while not rospy.is_shutdown():
        
        message = 'b%03dc%03dd%03de%03df%03d' %(b_top, c_starb, d_bottom , e_port, prop)
        #print message
        #write the message to the serial port
        try:
            serialPort.write(message)       
        except ValueError,  select.error:
            print 'write error'

        #print 'reading...' -   MAY NEED TO WAIT FOR BYTES IN INPUT BUFFER BEFORE ATTEMPTING TO READ
        startTime = datetime.datetime.now()
        try:
            feedback = serialPort.read(25)
        except ValueError:
            print 'read error'
        except select.error:
            print 'system call interrupted'
        #print (datetime.datetime.now() - startTime).microseconds
        #feedback = numpy.fromstring(feedback, dtype=numpy.uint8)      

        #data = serialPort.readline(size = None, eol = '\n')
        #print feedback
        process_feedback(feedback)
        
        #print '####################'
        
def process_feedback(feedback):
     global pub
     global prop_demand
    
     #print feedback[0:4]
        
        # outputs angle in degrees
     b_fb = -0.2073 * float(feedback[0:4]) + 100.07
     c_fb = 0.2088 * float(feedback[4:8]) - 101.92
     d_fb = 0.2055 * float(feedback[8:12]) - 100.37
     e_fb = -0.2117 * float(feedback[12:16]) + 103.03
     
     prop_current = (float(feedback[17:20])-512)*0.0763
     
     if prop_demand >= 10:
        prop_rpm = (float(feedback[21:24])*0.4183)-59.278
     else:
        prop_rpm = 0
        
     #print b_fb
     #print c_fb
     #print d_fb
     #print e_fb
        
     output = tail_feedback()
     output.bsp = b_top
     output.b = b_fb
     output.csp = c_starb
     output.c = c_fb
     output.dsp = d_bottom
     output.d = d_fb
     output.esp = e_port
     output.e = e_fb
     output.current = prop_current
     output.rpm = prop_rpm
     pub.publish(output)
     
     print 'Feedback', output
    


def shutdown():
    pubStatus.publish(nodeID = 2, status = False)
    message = 'b080c070d080e070f094'
    serialPort.flushOutput()
    serialPort.write(message)

            
            
################################################################
################################################################
if __name__ == '__main__':
    time.sleep(3) #Allow System to come Online    
    global pub
    global prop_demand
    global serialPort
    
    pub = rospy.Publisher('tail_output', tail_feedback)
    pubStatus = rospy.Publisher('status', status)
    
    prop_demand = 0
    
    rospy.init_node('tail_section')
    rospy.on_shutdown(shutdown)
    
    port_status = setupSerial()
    str = "Tailsection serial port status = %s. Port = %s" %(port_status, serialPort.portstr)
    
    rospy.Subscriber('tail_setpoints_horizontal', tail_setpoints, horizontal_callback)
    rospy.Subscriber('tail_setpoints_vertical', tail_setpoints, vertical_callback)
    rospy.Subscriber('prop_demand', Int8, prop_callback)

    
    if (port_status and tailAlive()) == True:    
        pubStatus.publish(nodeID = 2, status = True)
        rospy.loginfo("Tail section now online")
    else:
        pubStatus.publish(nodeID = 2, status = False)
    
    tail_section_loop()
 

